//
//  GUIcontroller.m
//  cocoaUDPServer
//
//  Created by Matt Roman on 7/16/08.
//  Copyright 2008 University of Oklahoma. All rights reserved.
//

#import "botSimulator.h"

// sr2 computer setup
//!!!!!!!!!!!! CHECK THE FILE PATH FOR THE SCRIPTS !!!!!!!!!!!!!!!!!!
#define CONFIGFILEPATH		@"~/Desktop/roverConfig.txt"
#define LOGFILEPATH			@"~/Desktop/loglog.txt"


#define SIMLOOPINTERVAL  0.005
#define REQUESTLOOPINTERVAL	0.1
#define LOGLOOPINTERVAL		5.0
#define NETWORKLOOPINTERVAL 1.0

// Recursive waypoint memory release
void clearWPS(wayPoint* wp){
	if(wp == NULL) return;
	if(wp->next != NULL) clearWPS(wp->next);
	wp->next = NULL;
	free(wp);
}

NSPoint makeNSPoint(CGFloat x,CGFloat y)
{
	NSPoint pt;
	pt.x = x;
	pt.y = y;
	return pt;
}

@interface botSimulator (Private)
- (void)updateSensors:(id)anObject;
- (void)simLoop;
- (void)networkLoop;
- (void)requestLoop;
- (void)logLoop;
- (void)setString:(NSString *)s;
- (void)sentAPlan:(unsigned)inAmount;
- (void)sentGoToWaypoint;
- (void)sentAPosition;
- (void)sentKonstants:(unsigned)inAmount;
- (void)sentMapName:(unsigned)inAmount;
- (void)writeKonstant:(NSString*)s value:(void*)num type:(NSString*)vType;
- (void)writeRoverKonstants;
- (void)readRoverKonstants;
- (void)killApp;
- (NSString *)stateString;
- (NSString *)errorString;
@end

wayPoint				*StartWaypoint;

@implementation botSimulator
- (void)awakeFromNib
{
	defaultKonstants();
	//sleep(10.0);
	//system("say 'Rover Ready please connect me'");
	/////////////////////////////////////////////////////////////////////////////////////////
	// read in konstants
	//[@"~/Desktop/" stringByExpandingTildeInPath];
	configFileName = [[NSString alloc] initWithString:[CONFIGFILEPATH stringByExpandingTildeInPath]];
	configFile = [NSFileHandle fileHandleForUpdatingAtPath:configFileName];
	if(configFile){
		[self readRoverKonstants];
	}
	else{
		[[NSFileManager defaultManager] createFileAtPath:configFileName contents:[@" " dataUsingEncoding:NSASCIIStringEncoding] attributes:nil];
		configFile = [NSFileHandle fileHandleForUpdatingAtPath:configFileName];
		[self writeRoverKonstants];
	}
	[configFile retain];
	/////////////////////////////////////////////////////////////////////////////////////////
	logFile = [[NSString alloc] initWithString:[LOGFILEPATH stringByExpandingTildeInPath]];
	logging = NO;	// rover parameters logging switch
	incommingDataList = NO;	// incomming data is not a waypoint list yet
	incommingGoTo = NO; // incomming data is not a waypoint to go to yet
	incommingPosition = NO; // incomming data is not a position
	incommingKonstants = NO; // incomming Konstant struct used for tuning
	incommingTerrainMap = NO; // incomming name of the terrain map used for simulation
	StartWaypoint = NULL; // pointer to the first waypoint in the linked list
	Joystick = NO;		// joystick switch
	stop = YES;			// rover should be in stop mode until a run command is given or joystick enabled
	updateRelays = NO;	// relay update switch
	[textToSend setStringValue:@"Run"];
	bzero(&sr2, sizeof(roverProperties));
	sr2.wp=NULL;
		
	///////////////////////
	// initialize all the lasers but do not connect yet wait for command to turn them on
	bzero(&bodyLaser, sizeof(laserScanner));
	[self startupBodyLaser];
	bzero(&panelLaser, sizeof(laserScanner));
	[self startupPanelLaser];
	bzero(&profileLaser, sizeof(laserScanner));
	[self startupProfileLaser];
	
	// start the server
	[self startServer];
	netData = [[NSMutableData alloc] init]; // network buffer array
	netString = [[NSMutableString alloc] init]; // network string output
	
	NSRect mainFrame;
	mainFrame.origin = makeNSPoint(700, 50);
	mainFrame.size.width = 700;
	mainFrame.size.height = 800;
	[mainWindow setFrame:mainFrame display:NO];
	[sim initWithFrame:[sim frame]];
	[sim getLasers:&bodyLaser panel:&panelLaser profile:&profileLaser];
	driveControl = NO;
	// setup the sensor update thread but dont start it until the app has finished launching 
	sendWhat = 0;
	sensorUpdateThread = [[NSThread alloc] initWithTarget:self selector:@selector(updateSensors:) object:nil];
	[textView setNeedsDisplay:YES];
}

- (void) applicationDidFinishLaunching:(NSNotification*)aNote
{
	logHandle = [NSFileHandle fileHandleForWritingAtPath:logFile];
	if(logHandle) {
		[logHandle seekToEndOfFile];
	}
	else{
		[[NSFileManager defaultManager] createFileAtPath:logFile contents:[@" " dataUsingEncoding:NSASCIIStringEncoding] attributes:nil];
		logHandle = [NSFileHandle fileHandleForWritingAtPath:logFile];
	}
	[logHandle retain];
	
	[sensorUpdateThread start];
}

- (BOOL)startServer
{
	server = [NetServer UDPServerSocket];
	[server retain];
	if(!server) return NO;
	[server setDelegate:self];
	if([server bindServerToPort:6570]) [textView insertText:@"Server started\n"];
	else return NO;
	return YES;
}

- (void) computeLaserConstants:(laserScanner*)scanner
{
	int i;
	int *oldData = scanner->rangeData;
	int *newData;
	
	scanner->alpha = ((135.0/384.0*(float)scanner->startPoint)-45.0)*PI/180.0;
	scanner->dAlpha = (((135.0/384.0)*(float)(scanner->stopPoint - scanner->startPoint))/(float)scanner->rayCount)*PI/180.0;
	newData = malloc(sizeof(int)*scanner->rayCount);
	for(i=0;i<scanner->rayCount;i++)newData[i]=4000;
	scanner->rangeData = newData;
	if(oldData)free(oldData);
}
- (void)startupBodyLaser
{
	bodyLaser.rayCount = 16;
	bodyLaser.startPoint = 208;
	bodyLaser.stopPoint = 560;
	bodyLaser.grouping = 22;
	bodyLaserDirection = -1;
	[self computeLaserConstants:&bodyLaser];
	sr2.bodyScanner = &bodyLaser;
}
- (void)startupPanelLaser
{
	panelLaser.rayCount = 16;
	panelLaser.startPoint = 208;
	panelLaser.stopPoint = 560;
	panelLaser.grouping = 22;
	panelLaserDirection = 1;
	[self computeLaserConstants:&panelLaser];
	sr2.panelScanner = &panelLaser;
}
- (void)startupProfileLaser
{
	profileLaser.rayCount = 16;
	profileLaser.startPoint = 384;
	profileLaser.stopPoint = 640;
	profileLaser.grouping = 16;
	profileLaserDirection = 1;
	[self computeLaserConstants:&profileLaser];
	sr2.profileScanner = &profileLaser;
}

//////////////////////////////////////////////////////////////////////////
// Thread and update loops
//////////////////////////////////////////////////////////////////////////
- (void)updateSensors:(id)anObject	// the sensor update thread
{
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];		// sensor reading timer
	NSTimer	*sensorTimer = [[NSTimer scheduledTimerWithTimeInterval:SIMLOOPINTERVAL
															 target:self
														   selector:@selector(simLoop)
														   userInfo:nil
															repeats:YES] retain];
	[[NSRunLoop currentRunLoop] addTimer:sensorTimer forMode:NSEventTrackingRunLoopMode];
																	// network writing timer
	NSTimer	*networkTimer = [[NSTimer scheduledTimerWithTimeInterval:NETWORKLOOPINTERVAL
															 target:self
														   selector:@selector(networkLoop)
														   userInfo:nil
															repeats:YES] retain];
	[[NSRunLoop currentRunLoop] addTimer:networkTimer forMode:NSEventTrackingRunLoopMode];
																	// sensor request update timer
	NSTimer	*requestTimer = [[NSTimer scheduledTimerWithTimeInterval:REQUESTLOOPINTERVAL
															  target:self
															selector:@selector(requestLoop)
															userInfo:nil
															 repeats:YES] retain];
	[[NSRunLoop currentRunLoop] addTimer:requestTimer forMode:NSEventTrackingRunLoopMode];
																	// start the logging timer
	NSTimer	*logTimer = [[NSTimer scheduledTimerWithTimeInterval:LOGLOOPINTERVAL
															  target:self
															selector:@selector(logLoop)
															userInfo:nil
															 repeats:YES] retain];
	[[NSRunLoop currentRunLoop] addTimer:logTimer forMode:NSEventTrackingRunLoopMode];
	CFRunLoopRun();
	[pool release];
	[NSThread exit];
}
- (void)logLoop
{
	if(!logging) return;
	NSLog(@"logging");
	NSString *logString = [[[NSString alloc] initWithFormat:@"%@,%d,%d,%0.12f,%0.12f,%f,%f,%0.3f,%0.3f,%0.3f,%f,%f,%f,%f,%f,%f,%d,%d,%d\n",
						   [[NSDate date] descriptionWithCalendarFormat:@"%Y%m%d%H%M%S" timeZone:nil locale:nil],
						   sr2.state,(sr2.wp)?sr2.wp->uuid : -1,
						   sr2.GPS.lon.dd,sr2.GPS.lat.dd,
						   sr2.position.x,sr2.position.y,
						   sr2.compass,sr2.tilt,sr2.roll,
						   sr2.voltage,
						   sr2.panelCurrent,sr2.electronicsCurrent,sr2.motorCurrent,
						   sr2.leftSpeed,sr2.rightSpeed,
						   sr2.leftStall,sr2.rightStall,sr2.turnStall] autorelease];
	[logHandle writeData:[logString dataUsingEncoding:NSASCIIStringEncoding]];
}

- (void)simLoop	// called on repeated timer from sensor update thread
{
	static float compassHead;
	float roverDistToWP;
	//float obstdist,head;
	
	if([sensorUpdateThread isCancelled]){	//if its time to exit stop running the secondary thread
		CFRunLoopStop([[NSRunLoop currentRunLoop] getCFRunLoop]); 
	}
	// update readings from compass
	compassHead = [sim heading];
	sr2.tilt = [sim pitch];
	sr2.roll = [sim roll];
	
	// update gps sensor values
	sr2.GPS.lon.dd = gpsCoordinates.lon.dd;
	sr2.GPS.lat.dd = gpsCoordinates.lat.dd;

	sr2.electronicsCurrent = 3;
	sr2.panelCurrent = 2.5;
	sr2.motorCurrent = 1;
	sr2.voltage = 15.5;
	sr2.xbctime = [sim xbcTime];
	updateRoverPosition(&sr2,[sim leftEncoder],[sim rightEncoder],compassHead);
		/////////////////////////////////////////////////////////////////////////////////////////////////
		// Main rover Navigation functions
		if(!Joystick && !stop){
			if(sr2.wp!=NULL){
				// more improved switch statement!!!!!!!!!!!!!!!!!!!!!!!!!
				switch ((int)(roverDistToWP = moveToWaypoint(&sr2))){
						case -1:
							[self WaypointFinished];
						break;
						case -2:
							[self WaypointSkipped];
						break;
						default:
						break;
				}
			}
			else{
				sr2.state=RSNoRemainingWaypoints;
				sr2.leftSpeed = 0;
				sr2.rightSpeed = 0;
				stop = YES;
			}
			if(sr2.error > RENone) stop = YES;
		}	 
		/////////////////////////////////////////////////////////////////////////////////////////////////
	
	//obstacleDistanceAndClearHeading(&sr2, &obstdist, &head);
	
	// update readings from lasers;
	if(0){//requestLaserUpdate){
		[sim bodyLaserUpdate];
		[sim panelLaserUpdate];
		[sim profileLaserUpdate];
		requestLaserUpdate = NO;
	}
	[StateOut setStringValue:[self stateString]];
	[ErrorOut setStringValue:[self errorString]];
}
- (void)requestLoop
{
	if(sr2.leftSpeed > rk.SPEEDLIMIT) sr2.leftSpeed = rk.SPEEDLIMIT;
	else if(sr2.leftSpeed < -rk.SPEEDLIMIT) sr2.leftSpeed = -rk.SPEEDLIMIT;
	if(sr2.rightSpeed > rk.SPEEDLIMIT) sr2.rightSpeed = rk.SPEEDLIMIT;
	else if(sr2.rightSpeed < -rk.SPEEDLIMIT) sr2.rightSpeed = -rk.SPEEDLIMIT;
	
	if(driveControl)[sim setSpeedLeft:sr2.leftSpeed right:sr2.rightSpeed];
	requestLaserUpdate = YES;
	if(updateRelays){
		updateRelays = NO;
	}
	
}

- (void)networkLoop
{
	int i=0;
	if([server numConnections] > 0){
		[netData setLength:0];
		//NSLog(@"case %d",sendWhat);
		switch (sendWhat) {
			case 0:
				[server sendString:@"SensorUpdate" encoding:NSASCIIStringEncoding];
				[netData appendBytes:&i length:sizeof(int)];
				[netData appendBytes:&sr2 length:sizeof(roverProperties)];
				if(1){ //[bodyLaser isConnected]
					i|=1;
					[netData appendBytes:bodyLaser.rangeData length:bodyLaser.rayCount*sizeof(int)];
				}
				if(1){ //[panelLaser isConnected]
					i|=2;
					[netData appendBytes:panelLaser.rangeData length:panelLaser.rayCount*sizeof(int)];
				}
				if(1){ //[profileLaser isConnected]
					i|=4;
					[netData appendBytes:profileLaser.rangeData length:profileLaser.rayCount*sizeof(int)];
				}
				if(i > 0) [netData replaceBytesInRange:NSMakeRange(0, sizeof(int)) withBytes:&i];
				[server sendData:netData];
				break;
			case 1:
				[server sendString:@"SensorSetup" encoding:NSASCIIStringEncoding];
				i=3;
				[netData appendBytes:&i length:sizeof(int)];
				[netData appendBytes:&bodyLaser length:sizeof(laserScanner)];
				[netData appendBytes:&panelLaser length:sizeof(laserScanner)];
				[netData appendBytes:&profileLaser length:sizeof(laserScanner)];
				[server sendData:netData];
				sendWhat = 0;
				break;
			case 2:
				[server sendString:netString encoding:NSASCIIStringEncoding];
				sendWhat = 0;
				break;
			case 3:
				[server sendString:@"Konstants" encoding:NSASCIIStringEncoding];
				[netData appendBytes:&rk length:sizeof(konstants)];
				[server sendData:netData];
				sendWhat = 1;
				break;
			case 4:
				if(sr2.wp != NULL){
					[server sendString:@"Waypoint" encoding:NSASCIIStringEncoding];
					[netData appendBytes:sr2.wp length:sizeof(wayPoint)];
					[server sendData:netData];
				}
				else sr2.state = RSNoRemainingWaypoints;
				sendWhat = 0;
			default:
				break;
		}
	}
}
- (void)netDataAvailable:(NetServer*)inNetServer amount:(unsigned)inAmount
{
	if(incommingDataList) [self sentAPlan:inAmount];
	else if(incommingGoTo) [self sentGoToWaypoint];
	else if(incommingPosition) [self sentAPosition];
	else if(incommingKonstants) [self sentKonstants:inAmount];
	else if(incommingTerrainMap) [self sentMapName:inAmount];
	else{
		NSString *input = [server readString:NSASCIIStringEncoding];
		[self valueForKey:input];
		if(![input hasPrefix:@"Joystick"])[textView insertText:[NSString stringWithFormat:@"%@\n",input]];
		[textView setNeedsDisplay:YES];
	}
}


- (void)netServerDisconnected:(NetServer*)inNetServer
{
	[textView insertText:@"Disconnected\n"];
	[server flushBuffers];
	incommingDataList = NO;
	incommingGoTo = NO;
	incommingPosition = NO;
}
- (void)netServerConnection:(NetServer*)inNetServer
{
	driveControl = YES;
	[textView insertText:@"A NEW connection\n"];
	sendWhat = 3;
}
- (id) valueForUndefinedKey:(NSString *)key
{
	[textView insertText:@"undefined function\n"];
	return nil;
}
//////////////////////////////////////////////////////////////////////////////
// Functions called from over the network
//////////////////////////////////////////////////////////////////////////////
- (void)SensorsSetup
{
	sendWhat = 1;
}
- (void)SensorStatus
{
	NSMutableString *statusString = [[[NSMutableString alloc] init] autorelease];
	[statusString appendString:@"Status\n"];
	[statusString appendString:@"HMR3000 running\n"];
	[statusString appendString:@"XBC running\n"];
	[statusString appendString:@"GPS running\n"];
	[statusString appendString:@"Body Laser ON\n"];
	[statusString appendString:@"Panel Laser ON\n"];
	[statusString appendString:@"Profile Laser ON\n"];
	[statusString appendString:@"End Status"];
	[self setString:statusString];
	//NSLog(@"size of status string %d",[statusString length]);
}
- (void)Konstants
{
	sendWhat = 3;
}
- (void)Stop
{
	stop = YES;
	sr2.state = RSStopped;
	sr2.leftSpeed = 0;
	sr2.rightSpeed = 0;
}
- (void)Run
{
	if(!Joystick)
	{
		stop = NO;
		sr2.state = RSMovingTowardsWaypoint;
		sr2.error = 0;
	}
}
- (void)JoystickOn
{
	Joystick = YES;
	stop = NO;
	sr2.state = RSInTeleopMode;
	sr2.leftSpeed = 0;
	sr2.rightSpeed = 0;
	[textView insertText:@"Joystick On\n"];
}
- (void)JoystickOff
{
	Joystick = NO;
	[self Stop];
	[textView insertText:@"Joystick Off\n"];
}
- (void)JoystickForward
{
	sr2.leftSpeed += rk.JOYINCREMENT;
	sr2.rightSpeed += rk.JOYINCREMENT;
}
- (void)JoystickBackward
{
	sr2.leftSpeed -= rk.JOYINCREMENT;
	sr2.rightSpeed -= rk.JOYINCREMENT;
}
- (void)JoystickLeft
{
	sr2.leftSpeed -= rk.JOYINCREMENT;
	sr2.rightSpeed += rk.JOYINCREMENT;
}
- (void)JoystickRight
{
	sr2.leftSpeed += rk.JOYINCREMENT;
	sr2.rightSpeed -= rk.JOYINCREMENT;
}
- (void)Sleeping
{
	[textView insertText:@"Client Going to Sleep\n"];
}
- (void)Sleep
{
	[self PowerDownLasers];
	[textView insertText:@"Rover going to sleep"];
}
- (void)Waking
{
	[textView insertText:@"Client Wakingup\n"];
	sendWhat = 3;
}
- (void)Wake
{
	system("say 'Rover Awake'");
	[self PowerUpLasers];
	sr2.state = RSWakingFromSleep;
	[self setString:@"RoverAwake"];
	sleep(5.0);
	sr2.state = RSMovingTowardsWaypoint;
	[textView insertText:@"Rover waking up"];
}
- (void)PowerUpLasers
{
	laserPower = YES;
	updateRelays = YES;
	[textView insertText:@"Body laser connected\n"];
	sr2.bodyScanner = &bodyLaser;
	sendWhat = 1;
	
	[textView insertText:@"Panel laser connected\n"];
	sr2.panelScanner = &panelLaser;
	sendWhat = 1;
	
	[textView insertText:@"Profile laser connected\n"];
	sr2.profileScanner = &profileLaser;
	sendWhat = 1;
}
- (void)PowerDownLasers
{
	laserPower = NO;
	updateRelays = YES;
	
	[self setString:@"LasersOff"];
}
- (void)ConnectXBC
{
	[textView insertText:@"XBC connected\n"];
}
- (void)ConnectGPS
{
	[textView insertText:@"GPS connected\n"];
}
- (void)ConnectCompass
{
	[textView insertText:@"HMR3000 connected\n"];
}
- (void)SystemShutdown
{
	system("say 'Shutting Down'");
	sleep(3.0);
	//system("open /Users/sr2/sr2/rovercontrol08/ScriptShutdown.app");
	[self killApp];
}
- (void)SystemRestart
{
	system("say 'Restarting System'");
	sleep(3.0);
	//system("open /Users/sr2/sr2/rovercontrol08/ScriptRestart.app");
	[self killApp];
}
- (void)RestartApp
{
	system("say 'Restarting App in 30 seconds not really'");
	sleep(3.0);
	//system("open /Users/sr2/sr2/rovercontrol08/ScriptRestartApp.app");
	[self killApp];
}
- (void)ResetRover
{
	[self Stop];
	sr2.position.x = 0;
	sr2.position.y = 0;
	sr2.position.z = 0;
	sr2.leftTicks = 0;
	sr2.rightTicks = 0;
	sr2.turnAvoidDirection = 0;
	sr2.odometer = 0;
	sr2.error = RENone;
	[self ClearWaypoints];
}
- (void)SetRover
{
	incommingPosition = YES;
}
- (void)UpdateKonstants
{
	incommingKonstants = YES;
}
- (void)PlanList
{
	incommingDataList = YES;
}
- (void)ClearWaypoints
{
	clearWPS(StartWaypoint);
	StartWaypoint = NULL;
}
- (void)GoToWaypoint
{
	incommingGoTo = YES;
}
- (void)WaypointFinished
{
	[self setString:@"WaypointFinished"];
	sr2.wp->state=WPStateOld;
	NSBeep();
	sr2.wp=sr2.wp->next;
	sr2.state=RSMovingTowardsWaypoint;
}
- (void)WaypointSkipped
{
	[self setString:@"WaypointSkipped"];
	sr2.wp->state=WPStateSkipped;
	NSBeep();
	sr2.wp=sr2.wp->next;
	sr2.state=RSMovingTowardsWaypoint;
}
- (void)WhatWaypoint
{
	sendWhat = 4;
}
- (void)Simulation
{
	incommingTerrainMap = YES;
}
//////////////////////////////////////////////////////////////////////////////
// receiving larger amounts of data
/////////////////////////////////////////////////////////////////////////////
- (void)sentAPlan:(unsigned)inAmount
{
	int currentCount = 0;
	static int	totalBytesNeeded = 0;
	int i=0;
	wayPoint *tempWaypoint;
	NSRange infoRange;
	NSData	*data;
	
	if(totalBytesNeeded == 0){
		data = [server readData:sizeof(int)];
		[data getBytes:&wpCount length:sizeof(int)];
		totalBytesNeeded = wpCount*sizeof(wayPoint);
	}
	totalBytesNeeded -= inAmount;
	
	if(totalBytesNeeded < 0){
		data = [server readData:sizeof(wayPoint)*wpCount];
		tempWaypoint = StartWaypoint = malloc(sizeof(wayPoint));
		while(currentCount < wpCount){	// exit when current waypoint count is greater than total # of waypoints
			infoRange.location = i;
			i+=sizeof(wayPoint);
			infoRange.length = sizeof(wayPoint);
			[data getBytes:tempWaypoint	range:infoRange]; // get the number of bytes off the data
			if(currentCount >= wpCount-1) {	// if the last waypoint is read
				tempWaypoint->next = NULL;	// set its next pointer to NULL
				totalBytesNeeded = 0;
				incommingDataList = NO;
				currentCount = 0;
				[self setString:@"PlanReceived"];
				sr2.wp = StartWaypoint;
				[sim setWPList:StartWaypoint];
				break;
			}
			tempWaypoint->next = malloc(sizeof(wayPoint));
			tempWaypoint = tempWaypoint->next;
			currentCount++;
		}
	}
}
- (void)sentGoToWaypoint
{
	int way;
	wayPoint *temp;
	NSData* goWaypoint = [server readData:sizeof(int)];
	[goWaypoint getBytes:&way length:sizeof(int)];
	temp = StartWaypoint;
	while(temp != NULL && temp->uuid != way) temp = temp->next;
	if(temp == NULL){
		[self setString:@"waypoint not found"];
	}
	else sr2.wp = temp;
	incommingGoTo = NO;
}
- (void)sentAPosition
{
	Point3D	pt;
	NSData* dataPoint = [server readData:sizeof(Point3D)];
	[dataPoint getBytes:&pt length:sizeof(Point3D)];
	sr2.position.x = pt.x;
	sr2.position.y = pt.y;
	sr2.position.z = pt.z;
	//NSLog(@"position %f,%f,%f",pt.x,pt.y,pt.z);
	incommingPosition = NO;
}
- (void)sentKonstants:(unsigned)inAmount
{
	static int bytesNeeded = 0;
	NSData		*data;
	
	if(bytesNeeded == 0) bytesNeeded = sizeof(konstants);
	
	bytesNeeded -= inAmount;
	if(bytesNeeded <= 0){
		data = [server readData:sizeof(konstants)];
		[data getBytes:&rk];
		bytesNeeded = 0;
		incommingKonstants = NO;
		[self writeRoverKonstants];
	}
}
- (void)sentMapName:(unsigned)inAmount
{
	static int bytesNeeded = 0;
	NSData		*data;
	static int nameLength;
	
	if(bytesNeeded == 0){
		data = [server readData:sizeof(int)];
		[data getBytes:&nameLength length:sizeof(int)];
		bytesNeeded = nameLength;
	} 
	bytesNeeded -= inAmount;
	
	if(bytesNeeded < 0){
		data = [server readData:nameLength];
		NSString *name = [[[NSString alloc] initWithData:data encoding:NSASCIIStringEncoding] autorelease];
		[sim loadTerrainFromFile:name];
		bytesNeeded = 0;
		incommingTerrainMap = NO;
	}
}
//////////////////////////////////////////////////////////////////////////////
// application configuration and commands
- (void)setString:(NSString *)s
{
	[netString setString:s];
	sendWhat = 2;
}
- (IBAction)sendOverServer:(id)sender
{
	[self setString:[textToSend stringValue]];
}
- (IBAction)sendOwnAction:(id)sender
{
	[self valueForKey:[textToSend stringValue]];
}

- (IBAction)beamSwitch:(id)sender
{
	[sender setState:![sender state]];
	[sim drawLaserBeams:[sender state]];
}
- (IBAction)remoteControlSelect:(id)sender
{
	[sender setState:![sender state]];
	driveControl = [sender state];
}
- (IBAction)waypointSim:(id)sender
{
	wayPoint *tempWaypoint;
	[self ClearWaypoints];
	StartWaypoint = malloc(sizeof(wayPoint));
	StartWaypoint->uuid = 1;
	StartWaypoint->position = MakePoint(30, 20, 0);
	StartWaypoint->science = 0;
	StartWaypoint->state = WPStateCurrent;
	StartWaypoint->next = malloc(sizeof(wayPoint));
	tempWaypoint = StartWaypoint->next;
	tempWaypoint->uuid = 2;
	tempWaypoint->position = MakePoint(40, 60, 0);
	tempWaypoint->science = 0;
	tempWaypoint->state = WPStateNew;
	tempWaypoint->next = NULL;
	sr2.wp = StartWaypoint;
	[sim setWPList:StartWaypoint];
	driveControl = YES;
}

- (NSString *)stateString
{
	NSString *stateString;
	
	switch (sr2.state) {
		case RSStopped:
			stateString = [NSString stringWithString:@"Rover Stopped"];
			break;
		case RSMovingTowardsWaypoint:
			stateString = [NSString stringWithString:@"Moving toward Waypoint"];
			break;
		case RSNoRemainingWaypoints:
			stateString = [NSString stringWithString:@"End of Waypoint list"];
			break;
		case RSGoingToSleep:
			stateString = [NSString stringWithString:@"Going to Sleep"];
			break;
		case RSWakingFromSleep:
			stateString = [NSString stringWithString:@"Waking from Sleep"];
			break;
		case RSDoingScience:
			stateString = [NSString stringWithString:@"Doing Science"];
			break;
		case RSCallingForHelp:
			stateString = [NSString stringWithString:@"Calling for Help"];
			break;
		case RSInTeleopMode:
			stateString = [NSString stringWithString:@"Joystick Mode"];
			break;
		case RSAvoidingNearbyObstacles:
			stateString = [NSString stringWithString:@"Avoiding Near Obstacles"];
			break;
		case RSAvoidingDistantObstacles:
			stateString = [NSString stringWithString:@"Avoiding Far Obstacles"];
			break;
		case RSReachedWaypoint:
			stateString = [NSString stringWithString:@"Waypoint Reached"];
			NSBeep();
			break;
		case RSGoingPastObstacles:
			stateString = [NSString stringWithString:@"Driving past an Obstacle"];
			break;
		default:
			stateString = [NSString stringWithString:@"Unknown State"];
			break;
	}
	return stateString;
}
- (NSString *)errorString
{
	NSString *errorString;
	
	switch (sr2.error) {
		case RENone:
			errorString = [NSString stringWithString:@"-"];
			break;
		case REPosition:
			errorString = [NSString stringWithString:@"Teleport position error"];
			break;
		case REStall:
			errorString = [NSString stringWithString:@"Stall error"];
			break;
		case REProgress:
			errorString = [NSString stringWithString:@"Progress Error"];
			break;
		case REPitch:
			errorString = [NSString stringWithString:@"Pitch Error"];
			break;
		case RERoll:
			errorString = [NSString stringWithString:@"Roll Error"];
			break;
		default:
			errorString = [NSString stringWithString:@"Unknown Error"];
			break;
	}
	return errorString;
}

- (void)writeKonstant:(NSString*)s value:(void*)num type:(NSString*)vType
{
	NSString *stng;
	if([vType isEqual:@"int"]) stng = [[NSString alloc] initWithFormat:@"%@=%d\n",s,*(int*)num];
	else if([vType isEqual:@"float"]) stng = [[NSString alloc] initWithFormat:@"%@=%0.8f\n",s,*(float*)num];
	//else if([vType isEqual:@"string"]) stng = [[NSString alloc] initWithFormat:@"%@%@\n",s,(char*)num];
	
	[configFile writeData:[stng dataUsingEncoding:NSASCIIStringEncoding]];
}

- (void)writeRoverKonstants
{
	unsigned long long fileoffset = [configFile offsetInFile];
	[configFile seekToFileOffset:0];
	[configFile writeData:[@"ROVER CONFIGURATION PARAMETERS\n" dataUsingEncoding:NSASCIIStringEncoding]];
	[self writeKonstant:@"rk.DECLINATION" value:&rk.DECLINATION type:@"float"];
	[self writeKonstant:@"rk.WHEELDIAM" value:&rk.WHEELDIAM type:@"float"];
	[self writeKonstant:@"rk.LTICSPERREV" value:&rk.LTICSPERREV type:@"int"];
	[self writeKonstant:@"rk.RTICSPERREV" value:&rk.RTICSPERREV type:@"int"];
	[self writeKonstant:@"rk.ROVERWIDTH" value:&rk.ROVERWIDTH type:@"float"];
	[self writeKonstant:@"rk.ROVERWIDTHINT" value:&rk.ROVERWIDTHINT type:@"int"];
	[self writeKonstant:@"rk.TELEPORTDISTANCE" value:&rk.TELEPORTDISTANCE type:@"float"];
	[self writeKonstant:@"rk.CLOSEENOUGH" value:&rk.CLOSEENOUGH type:@"float"];
	[self writeKonstant:@"rk.NOPROGRESSTIME" value:&rk.NOPROGRESSTIME type:@"int"];
	[self writeKonstant:@"rk.STALLPERCENTAGE" value:&rk.STALLPERCENTAGE type:@"float"];
	[self writeKonstant:@"rk.TURNDIFF" value:&rk.TURNDIFF type:@"float"];
	[self writeKonstant:@"rk.POINTTURNANGLE" value:&rk.POINTTURNANGLE type:@"float"];
	[self writeKonstant:@"rk.POINTTURNSPEED" value:&rk.POINTTURNSPEED type:@"float"];
	[self writeKonstant:@"rk.PTSTALLTIME" value:&rk.PTSTALLTIME type:@"int"];
	[self writeKonstant:@"rk.TURNMULT" value:&rk.TURNMULT type:@"float"];
	[self writeKonstant:@"rk.NEEDTURNCORRECTION" value:&rk.NEEDTURNCORRECTION type:@"float"];
	[self writeKonstant:@"rk.TURNACCURACYLIMIT" value:&rk.TURNACCURACYLIMIT type:@"float"];
	[self writeKonstant:@"rk.ROVERCRUISESPEED" value:&rk.ROVERCRUISESPEED type:@"int"];
	[self writeKonstant:@"rk.TURNINCREMENT" value:&rk.TURNINCREMENT type:@"float"];
	[self writeKonstant:@"rk.JOYINCREMENT" value:&rk.JOYINCREMENT type:@"int"];
	[self writeKonstant:@"rk.SPEEDLIMIT" value:&rk.SPEEDLIMIT type:@"int"];
	[self writeKonstant:@"rk.BODYDIST" value:&rk.BODYDIST type:@"int"];
	[self writeKonstant:@"rk.PROFILEHEIGHT" value:&rk.PROFILEHEIGHT type:@"float"];
	[self writeKonstant:@"rk.PANELHEIGHT" value:&rk.PANELHEIGHT type:@"float"];
	[self writeKonstant:@"rk.PROFILEOBSTACLEMAX" value:&rk.PROFILEOBSTACLEMAX type:@"float"];
	[self writeKonstant:@"rk.PROFILEOBSTACLEMIN" value:&rk.PROFILEOBSTACLEMIN type:@"float"];
	[self writeKonstant:@"rk.PROFILESEG" value:&rk.PROFILESEG type:@"int"];
	[self writeKonstant:@"rk.PANELSEGMENTS" value:&rk.PANELSEGMENTS type:@"int"];
	[self writeKonstant:@"rk.PROFILEINCR" value:&rk.PROFILEINCR type:@"float"];
	[self writeKonstant:@"rk.BODYINCR" value:&rk.BODYINCR type:@"float"];
	[self writeKonstant:@"rk.PANELINCR" value:&rk.PANELINCR type:@"float"];
	[self writeKonstant:@"rk.MAXPITCH" value:&rk.MAXPITCH type:@"float"];
	[self writeKonstant:@"rk.MAXROLL" value:&rk.MAXROLL type:@"float"];
	[self writeKonstant:@"rk.BODYTOOCLOSE" value:&rk.BODYTOOCLOSE type:@"float"];
	[self writeKonstant:@"rk.FOROFFSETWHEELS" value:&rk.FOROFFSETWHEELS type:@"float"];
	[self writeKonstant:@"rk.FOROFFSETBODYLASER" value:&rk.FOROFFSETBODYLASER type:@"float"];
	[self writeKonstant:@"rk.FOROFFSETPROFILE" value:&rk.FOROFFSETPROFILE type:@"float"];
	[self writeKonstant:@"rk.FOROFFSETPANELLASER" value:&rk.FOROFFSETPANELLASER type:@"float"];
	[self writeKonstant:@"rk.FORPANELGROUNDINTERSECT" value:&rk.FORPANELGROUNDINTERSECT type:@"float"];
	[self writeKonstant:@"rk.MAXDISTMAP" value:&rk.MAXDISTMAP type:@"int"];
	[self writeKonstant:@"rk.NAPTIME" value:&rk.NAPTIME type:@"int"];
	[configFile writeData:[@"\n\n" dataUsingEncoding:NSASCIIStringEncoding]];
	[configFile seekToFileOffset:fileoffset];
}
- (void)readRoverKonstants
{
	[configFile seekToFileOffset:0];
	NSString *fileData = [[NSString alloc] initWithContentsOfFile:configFileName encoding:NSASCIIStringEncoding error:NULL];
	NSRange tempRange = [fileData rangeOfString:@"\n\n"];
	tempRange.length = tempRange.location;
	tempRange.location = 0;
	NSArray *paramKonstants = [[fileData substringWithRange:tempRange] componentsSeparatedByString:@"\n"];
	rk.DECLINATION = [[[[paramKonstants objectAtIndex:1] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.WHEELDIAM = [[[[paramKonstants objectAtIndex:2] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.LTICSPERREV = [[[[paramKonstants objectAtIndex:3] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.RTICSPERREV = [[[[paramKonstants objectAtIndex:4] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.ROVERWIDTH = [[[[paramKonstants objectAtIndex:5] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.ROVERWIDTHINT = [[[[paramKonstants objectAtIndex:6] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.TELEPORTDISTANCE = [[[[paramKonstants objectAtIndex:7] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.CLOSEENOUGH = [[[[paramKonstants objectAtIndex:8] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.NOPROGRESSTIME = [[[[paramKonstants objectAtIndex:9] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.STALLPERCENTAGE = [[[[paramKonstants objectAtIndex:10] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.TURNDIFF = [[[[paramKonstants objectAtIndex:11] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.POINTTURNANGLE = [[[[paramKonstants objectAtIndex:12] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.POINTTURNSPEED = [[[[paramKonstants objectAtIndex:13] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.PTSTALLTIME = [[[[paramKonstants objectAtIndex:14] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.TURNMULT = [[[[paramKonstants objectAtIndex:15] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.NEEDTURNCORRECTION = [[[[paramKonstants objectAtIndex:16] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.TURNACCURACYLIMIT = [[[[paramKonstants objectAtIndex:17] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.ROVERCRUISESPEED = [[[[paramKonstants objectAtIndex:18] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.TURNINCREMENT = [[[[paramKonstants objectAtIndex:19] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.JOYINCREMENT = [[[[paramKonstants objectAtIndex:20] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.SPEEDLIMIT = [[[[paramKonstants objectAtIndex:21] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.BODYDIST = [[[[paramKonstants objectAtIndex:22] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.PROFILEHEIGHT = [[[[paramKonstants objectAtIndex:23] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.PANELHEIGHT = [[[[paramKonstants objectAtIndex:24] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.PROFILEOBSTACLEMAX = [[[[paramKonstants objectAtIndex:25] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.PROFILEOBSTACLEMIN = [[[[paramKonstants objectAtIndex:26] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.PROFILESEG = [[[[paramKonstants objectAtIndex:27] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.PANELSEGMENTS = [[[[paramKonstants objectAtIndex:28] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.PROFILEINCR = [[[[paramKonstants objectAtIndex:29] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.BODYINCR = [[[[paramKonstants objectAtIndex:30] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.PANELINCR = [[[[paramKonstants objectAtIndex:31] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.MAXPITCH = [[[[paramKonstants objectAtIndex:32] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.MAXROLL = [[[[paramKonstants objectAtIndex:33] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.BODYTOOCLOSE = [[[[paramKonstants objectAtIndex:34] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.FOROFFSETWHEELS = [[[[paramKonstants objectAtIndex:35] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.FOROFFSETBODYLASER = [[[[paramKonstants objectAtIndex:36] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.FOROFFSETPROFILE = [[[[paramKonstants objectAtIndex:37] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.FOROFFSETPANELLASER = [[[[paramKonstants objectAtIndex:38] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.FORPANELGROUNDINTERSECT = [[[[paramKonstants objectAtIndex:39] componentsSeparatedByString:@"="] objectAtIndex:1] floatValue];
	rk.MAXDISTMAP = [[[[paramKonstants objectAtIndex:40] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
	rk.NAPTIME = [[[[paramKonstants objectAtIndex:41] componentsSeparatedByString:@"="] objectAtIndex:1] intValue];
}
	 
- (void)windowWillClose:(NSNotification*)note
{
	[self killApp];
}
- (void)killApp
{
	[server sendString:@"GoodBye" encoding:NSASCIIStringEncoding];
	NSLog(@"goodbye");
	logging = NO;
	[logHandle closeFile];
	[sensorUpdateThread cancel];
	[server close];
	if(configFile != nil){
		[configFile closeFile];
		configFile == nil;
	}
	[NSApp terminate:nil];
}

@end

